#include"ros/ros.h"
#include"tello_show/Series.h"
#include<iostream>
#include<stdlib.h>
#include<string.h>

using namespace std;

double pi = 3.1415926;
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");

    //1.初始化ros节点
    ros::init(argc,argv,"position_series");

    //2.创建ros句柄
    ros::NodeHandle nh;

    //3.创建发布者对象
    ros::Publisher pub1 = nh.advertise<tello_show::Series>("pos_series1",1000);
    ros::Publisher pub2 = nh.advertise<tello_show::Series>("pos_series2",1000);

    //4.组织被发布的消息并编写发布逻辑
    tello_show::Series p1,p2;
    p1.pos_series_x = {0.5,0.5,0,-0.5,-0.5,-0.5,0,0.5,0.5,0};
    p1.pos_series_y = {0,0.5,0.5,0.5,0,-0.5,-0.5,-0.5,0,0};
    // p1.pos_series_x = {1,cos(pi/6),cos(pi/3),cos(pi/2),cos(pi*2/3),cos(pi*5/6),cos(pi),cos(pi*4/3),cos(pi*5/3),1};
    // p1.pos_series_y = {1,sin(pi/6),sin(pi/3),sin(pi/2),sin(pi*2/3),sin(pi*5/6),sin(pi),sin(pi*4/3),sin(pi*5/3),1};
    p2.pos_series_x = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1.0,-1.1};
    p2.pos_series_y = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1.0,-1.1};

    ros::Rate rate(0.05);
        //发布频率为20s一次
    ros::Duration(2.0).sleep();
        //发布前进行2s休眠
    int count = 0;
    while(ros::ok())
    {
        pub1.publish(p1);
        pub2.publish(p2);
        rate.sleep();
        count++;
        ros::spinOnce();
    }

    return 0;
}
